/**
 * edurender
 * er_math.c
 * (c) 2009 J. Brad Byrd
 */

#include "er_math.h"

#include <string.h>

ERMatrix4x4f *erIdentityMatrix(ERMatrix4x4f *out)
{
	memset(out, 0, sizeof(ERfloat) * 16);
	
	out->_00 = 1.0f;
	out->_11 = 1.0f;
	out->_22 = 1.0f;
	out->_33 = 1.0f;

	return out;
}

ERMatrix4x4f *erMulMatrix(ERMatrix4x4f *out, const ERMatrix4x4f *a, const ERMatrix4x4f *b)
{
	ERMatrix4x4f result;
	ERuint row, col;
	float *dst = &result._00;
	const float *srca = &a->_00;
	const float *srcb = &b->_00;

	for (row = 0; row < 4; row++)
	{
		ERuint row4 = row * 4;
		
		for (col = 0; col < 4; col++)
		{
			*dst++ =
				srca[row4 + 0] * srcb[0  + col] +
				srca[row4 + 1] * srcb[4  + col] +
				srca[row4 + 2] * srcb[8  + col] +
				srca[row4 + 3] * srcb[12 + col];
		}
	}
	
	*out = result;
	
	return out;
}

ERMatrix4x4f *erTransposeMatrix(ERMatrix4x4f *out, const ERMatrix4x4f *m)
{
	ERMatrix4x4f result;
	ERuint row, col;
	float *dst = &result._00;
	const float *src = &m->_00;
	
	for (col = 0; col < 4; col++)
		for (row = 0; row < 4; row++)
			dst[col * 4 + row] = src[row * 4 + col];

	*out = result;

	return out;
}

ERVector3f *erTransformProjectPoint3(ERVector3f *out, const ERMatrix4x4f *m, const ERVector3f *p)
{
	ERVector3f result;
	ERfloat invW = 1.0f / (p->x * m->_30 + p->y * m->_31 + p->z * m->_32 + m->_33);
	
	result.x = (p->x * m->_00 + p->y * m->_01 + p->z * m->_02 + m->_03) * invW;
	result.y = (p->x * m->_10 + p->y * m->_11 + p->z * m->_12 + m->_13) * invW;
	result.z = (p->x * m->_20 + p->y * m->_21 + p->z * m->_22 + m->_23) * invW;

	*out = result;

	return out;
}

ERVector4f *erTransformPoint3(ERVector4f *out, const ERMatrix4x4f *m, const ERVector3f *p)
{
	ERVector4f result;
	
	result.x = (p->x * m->_00 + p->y * m->_01 + p->z * m->_02 + m->_03);
	result.y = (p->x * m->_10 + p->y * m->_11 + p->z * m->_12 + m->_13);
	result.z = (p->x * m->_20 + p->y * m->_21 + p->z * m->_22 + m->_23);
	result.w = (p->x * m->_30 + p->y * m->_31 + p->z * m->_32 + m->_33);
	
	*out = result;
	
	return out;
}
